#include <stdlib.h>
#include "kernel.h"
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "os.h"
#include "LED_Test.h"

 
int timer16Started = 0;
TASK_HANDLER* currTimerTask = NULL;
 
/****************************************************************************************/
 
void initNextTask() {
    int task;
    if (PERIODIC_count == 0) {
       return;
    } // end-if
    if (PPP[nextPPP]==0) {
       nextPPP = 0;
    } // end-if
    
    while(&(Task_List[PPP[nextPPP]].state)==DEAD) {
       nextPPP+=2;
       if (PPP[nextPPP]==0) {
          nextPPP = 0;
       } // end-if
    } // end-while
    
    task = (PPP[nextPPP]);
    if (task==0) {
       nextPPP = 0;
       task = PPP[0];
    } // end-if
    
    // Test and verify the next task isn't FCFS -- if it is, we need to signal an error
    if (Task_List[task].level == FCFS) {
       // We weren't expecting an FCFS task here.
       OS_Abort();
    } // end-if
    
    //OCR1A = PPP[nextPPP+1]*TIMER_FACTOR*PROCESSOR_SPEED;
    currTimerTask = &(Task_List[PPP[nextPPP]]);
    currTimerTask->state = READY;
    periodic_task = PPP[nextPPP];
    nextPPP+=2;
    return;
 } // end-function
 
 
/****************************************************************************************/
 
 ISR (TIMER1_COMPA_vect) {


	OCR1A += 40000; //5ms @8Mhz
	if (CurrentTask == currTimerTask) {
       // The task has overflowed it's allotted time.  Abort!
  
       OS_Abort();
    } else {
       // Set-up and call the next task
       // Figure out what the next task is
       // Reset the output compare register OCR1A to the time for the next task
       initNextTask();
       
       // Call the next task
       Task_Next();
    } // end-if
 } // end-function
 
/****************************************************************************************/
 
 void initTimer() { 
    if (timer16Started) return;
    
    // Don't turn on interrupts just yet -- they need to be enabled when the first periodic task starts.
    
    TIMSK1 = 0;                                              // Disable timer interrupts 
    TCCR1A = 0;                                              // Configure the initial timer properties (CTC)
    TCCR1B = (_BV(WGM12)) | (_BV(CS12)) | (_BV(CS10));       // Configure the initial timer properties (clk/1024)
    TCNT1  = 0;
    
    timer16Started = 1;                                      // Flag the timer as started.
 } // end-function 
 
/****************************************************************************************/
 
 void startTimer(void) {
    if (!timer16Started) initTimer();
    
    OCR1A = PPP[(nextPPP+1)]*TIMER_FACTOR*PROCESSOR_SPEED;   // Init the counter to the first time.
    periodic_task = PPP[nextPPP];                            // Set the next periodic task
    //Pick the task, and use it to initialize the timer
    currTimerTask = &(Task_List[PPP[nextPPP]]);
    
    nextPPP+=2;                                              // Increment to the next task
    
    TIMSK1 |= _BV(OCIE1A);                                   // Enable interrupts    
    TCNT1 = 0;
 } // end-function
/****************************************************************************************/
 
 void stopTimer(void) {
    TIMSK1 &= !_BV(OCIE1A);                                  // Disable interrupts
 } // end-function
 /****************************************************************************************/
